#requires the robot team module
require 'control_lib/level2/main/robot_team.rb'

#requires the robot teammap module
require 'control_lib/level2/support/teammap.rb'




team1= RobotTeam.new("searchteam","10.1.157.59",3000)
team1.addrobot("USARBot.P2AT","-6.0,-2.0,1.8","robot1")
team1.addlog("robot1")


sleep(1)

map=Teammap.new(team1)
map.autoupdatemap(false)
map.autologmap(true)


team1.robotno.each{|robot|
  robot.colguardmode
}



cbot=team1.robotno[0]
#1=left 2=right 
movearray=[1,1,2,2,1,1,2,2,2]

cbot.smartright(6.28/4)
#Main Loop
oldtime=Time.now
totaltime=0.0
while(totaltime<=60.0*10.0)
totaltime=(Time.now-oldtime)+totaltime
oldtime=Time.now
puts "Time Taken:#{totaltime}"
map.logins
if(!cbot.col_frontcheck)
  cbot.smartfor_acc(0.5,2.0)  
  sleep(0.5)
  1.times{
  sleep(0.05) 
  #map.ruby_maplaserbayes_stand(cbot,0.02)
  #map.ruby_maplaserbayes(cbot)
  # map.ruby_maplaserincrement(cbot)
  # map.ruby_maplaseroverwrite(cbot)
  puts  "insx=#{cbot.ins[0]} truex=#{cbot.truepos[0]} diffx=#{(cbot.ins[0].to_f-cbot.truepos[0].to_f).abs}"
  puts  "insy=#{cbot.ins[1]} truey=#{cbot.truepos[1]} diffy=#{(cbot.ins[1].to_f-cbot.truepos[1].to_f).abs}"
  puts  "insz=#{cbot.ins[2]} truez=#{cbot.truepos[2]} diffz=#{(cbot.ins[2].to_f-cbot.truepos[2].to_f).abs}"
  puts  "insang=#{cbot.ins[5]} trueang=#{cbot.truepos[5]} diffrad=#{(cbot.ins[5].to_f-cbot.truepos[5].to_f).abs}"
}  
else
  if (!movearray.nil?)
  dir=movearray.shift
   if dir==1
      puts "moving left"
      cbot.smartleft(6.28/4)
   else
      puts "moving right"
      cbot.smartright(6.28/4) 
   end
  else
    cbot.smartleft(6.28/4)
  end
end
  
end
 
 


sleep(20)